namespace Microsoft.Robotics.Navigation.Runtime.UnitTests
{
    using System;
    using System.Collections.Generic;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Unit tests for inclusion zones
    /// </summary>
    [TestClass]
    public class InclusionZoneTests
    {
        /// <summary>
        /// Tests if GenerateInclusionZoneScanLinesThrowsIfRadiusIsNegative
        /// </summary>
        [Priority(0)]
        [TestMethod, ExpectedException(typeof(ArgumentException))]
        [TestCategory("Unit")]
        public void GenerateInclusionZoneScanLinesThrowsIfRadiusIsNegative()
        {
            InclusionZoneMarker map = new InclusionZoneMarker();

            int zeroRadius = -1;
            double goodFov = 0.5;
            map.GenerateInclusionZoneScanLines(new Pose2D(0, 0, 0), goodFov, zeroRadius);
        }

        /// <summary>
        /// Tests if GenerateInclusionZoneScanLinesReturnEmptyCollectionIfRadiusIsZero
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GenerateInclusionZoneScanLinesReturnEmptyCollectionIfRadiusIsZero()
        {
            InclusionZoneMarker map = new InclusionZoneMarker();

            int zeroRadius = 0;
            double goodFov = 0.5;
            var scanLines = map.GenerateInclusionZoneScanLines(new Pose2D(0, 0, 0), goodFov, zeroRadius);

            Assert.AreEqual(0, scanLines.Count);
        }

        /// <summary>
        /// Tests if GenerateInclusionZoneScanLinesThrowsIfFOVIsTooLarge
        /// </summary>
        [Priority(0)]
        [TestMethod, ExpectedException(typeof(ArgumentException))]
        [TestCategory("Unit")]
        public void GenerateInclusionZoneScanLinesThrowsIfFOVIsTooLarge()
        {
            InclusionZoneMarker map = new InclusionZoneMarker();

            int goodRadius = 5;
            double largeFov = 2 * Math.PI;
            map.GenerateInclusionZoneScanLines(new Pose2D(0, 0, 0), largeFov, goodRadius);
        }

        /// <summary>
        /// Tests if GenerateInclusionMapThrowsIfFOVIsTooSmall
        /// </summary>
        [Priority(0)]
        [TestMethod, ExpectedException(typeof(ArgumentException))]
        [TestCategory("Unit")]
        public void GenerateInclusionZoneScanLinesThrowsIfFOVIsTooSmall()
        {
            InclusionZoneMarker map = new InclusionZoneMarker();

            int goodRadius = 5;
            double zeroFov = 0;
            map.GenerateInclusionZoneScanLines(new Pose2D(0, 0, 0), zeroFov, goodRadius);
        }

        /// <summary>
        /// Tests if AllPixelsInInclusionZoneAreValid
        /// Note, this is a slow method (2-3sec), as we traverse a large number of combinations of FOV and range values
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void AllPixelsInInclusionZoneAreValid()
        {
            InclusionZoneMarker map = new InclusionZoneMarker();

            double thetaRangeMin = -Math.PI * 2;
            double thetaRangeMax = Math.PI * 2;

            double fovMin = 0.5;
            double fovMax = Math.PI - 0.01;

            // Some realistic origins
            int originX = 100;
            int originY = 1000;

            int radius = 20;

            double thetaStep = Math.PI / 20;
            double fovStep = Math.PI / 20;

            for (double heading = thetaRangeMin; heading <= thetaRangeMax; heading += thetaStep)
            {
                Pose2D pose = new Pose2D(originX, originY, heading);

                for (double fov = fovMin; fov <= fovMax; fov += fovStep)
                {
                    List<FastTuple<int, int, int>> inclusionZone = map.GenerateInclusionZoneScanLines(pose, fov, radius);
                    
                    bool success = this.ValidateIncludedPixels(inclusionZone, pose, fov, radius);

                    Assert.IsTrue(success, "Validation failed for heading:{0} fov:{1}", heading, fov); 
                }
            }
        }

        /// <summary>
        /// Validate included pixels using slow but simple method
        /// </summary>
        /// <param name="scanLines">Array to validate</param>
        /// <param name="pose">The pose</param>
        /// <param name="fov">Field of view</param>
        /// <param name="radius">The radius</param>
        /// <returns>true if all pixels in test set match verification set and vice versa</returns>
        private bool ValidateIncludedPixels(List<FastTuple<int, int, int>> scanLines, Pose2D pose, double fov, int radius)
        {
            List<Tuple<int, int>> testSet = this.GenerateTestPixelSetFromScanLines(scanLines);

            List<Tuple<int, int>> validationSet = this.GenerateValidationSetUsingSimpleTrig(ref pose, fov, radius);

            bool result = this.VerifyAllPixelsFromValidationSetAreInTestSet(validationSet, testSet);

            if (result)
            {
                result = this.VerifyAllPixelsInTestSetAreInValidationSet(validationSet, testSet);
            }

            return result;
        }

        /// <summary>
        /// Slow but simple inclusion zone detector to validate our faster method
        /// </summary>
        /// <param name="pose">Test pose</param>
        /// <param name="fov">Field of view</param>
        /// <param name="radius">The range</param>
        /// <returns>All pixels in inclusion zone</returns>
        private List<Tuple<int, int>> GenerateValidationSetUsingSimpleTrig(ref Pose2D pose, double fov, int radius)
        {
            Pose2D line1 = new Pose2D(pose.X, pose.Y, pose.Heading - fov / 2);
            Pose2D line2 = new Pose2D(pose.X, pose.Y, pose.Heading + fov / 2);

            List<Tuple<int, int>> validationSet = new List<Tuple<int, int>>();

            for (int row = -radius; row <= radius; ++row)
            {
                for (int col = -radius; col <= radius; ++col)
                {
                    double x = col;
                    double y = -row;
                    double tanAlpha = (double)(y / x);
                    double pointAngle = Math.Atan(tanAlpha);

                    bool include = false;

                    if (x == 0 && y == 0)
                    {
                        // center always included
                        include = true;
                    }
                    else if (x >= 0 && y >= 0)
                    {
                        if ((line1.Heading <= pointAngle && line1.Heading >= -Math.PI + pointAngle) &&
                             (line2.Heading >= pointAngle || line2.Heading <= -Math.PI + pointAngle))
                        {
                            include = true;
                        }
                    }
                    else if (x <= 0 && y >= 0)
                    {
                        if ((line1.Heading <= Math.PI + pointAngle && line1.Heading >= pointAngle) &&
                             (line2.Heading >= Math.PI + pointAngle || line2.Heading <= pointAngle))
                        {
                            include = true;
                        }
                    }
                    else if (x <= 0 && y <= 0 && pointAngle >= 0)
                    {
                        if ((line1.Heading >= pointAngle || line1.Heading <= -Math.PI + pointAngle) &&
                             (line2.Heading >= pointAngle - Math.PI && line2.Heading <= pointAngle))
                        {
                            include = true;
                        }
                    }
                    else if (x >= 0 && y <= 0)
                    {
                        if ((line1.Heading <= pointAngle || line1.Heading >= Math.PI + pointAngle) &&
                             (line2.Heading >= pointAngle && line2.Heading <= Math.PI + pointAngle))
                        {
                            include = true;
                        }
                    }

                    if ((x * x + y * y <= radius * radius) && include)
                    {
                        validationSet.Add(new Tuple<int, int>((int)(col + pose.X), (int)(pose.Y - row)));
                    }
                }
            }

            return validationSet;
        }

        /// <summary>
        /// Generates list of all all pixels included in the generated scan line array
        /// </summary>
        /// <param name="scanLines">Scan lines as produced by the algorithm we are testing</param>
        /// <returns>List of all all pixels (x, y coordinates adjusted for origin) included in the generated scan line array</returns>
        private List<Tuple<int, int>> GenerateTestPixelSetFromScanLines(List<FastTuple<int, int, int>> scanLines)
        {
            List<Tuple<int, int>> testSet = new List<Tuple<int, int>>();

            foreach (var scanLineToTest in scanLines)
            {
                int row = scanLineToTest.Item3;

                for (int col = scanLineToTest.Item1; col < scanLineToTest.Item2; ++col)
                {
                    testSet.Add(new Tuple<int, int>(col, row));
                }
            }

            return testSet;
        }

        /// <summary>
        /// Verify that all pixels generated by the test method are found in the array generated by main algorithm
        /// </summary>
        /// <param name="validationSet">Array of pixels generated by verification algorithm</param>
        /// <param name="testSet">Array of pixels generated by the algorithm we are verifying</param>
        /// <returns>true if pixels match, false otherwise</returns>
        private bool VerifyAllPixelsFromValidationSetAreInTestSet(List<Tuple<int, int>> validationSet, List<Tuple<int, int>> testSet)
        {
            foreach (var t in validationSet)
            {
                var found = testSet.Find(
                    c => (t.Item1 == c.Item1 && t.Item2 == c.Item2));

                if (null == found)
                {
                    return false;
                }
            }

            return true;
        }

        /// <summary>
        /// Verify that all pixels generated by the main algorithm are found in the array generated by a differently implemented test method
        /// </summary>
        /// <param name="validationSet">Array of pixels generated by verification algorithm</param>
        /// <param name="testSet">Array of pixels generated by the algorithm we are verifying</param>
        /// <returns>true if pixels match, false otherwise</returns>
        private bool VerifyAllPixelsInTestSetAreInValidationSet(List<Tuple<int, int>> validationSet, List<Tuple<int, int>> testSet)
        {
            foreach (var t in testSet)
            {
                var found = validationSet.Find(
                    c => (t.Item1 == c.Item1 && t.Item2 == c.Item2));

                if (null == found)
                {
                    found = validationSet.Find(c =>
                        ((c.Item1 == t.Item1 - 1 && c.Item2 == t.Item2) ||
                            (c.Item1 == t.Item1 && c.Item2 == t.Item2 - 1) ||
                            (c.Item1 == t.Item1 + 1 && c.Item2 == t.Item2) ||
                            (c.Item1 == t.Item1 + 1 && c.Item2 == t.Item2 + 1) ||
                            (c.Item1 == t.Item1 - 1 && c.Item2 == t.Item2 - 1) ||
                            (c.Item1 == t.Item1 - 1 && c.Item2 == t.Item2 + 1) ||
                            (c.Item1 == t.Item1 + 1 && c.Item2 == t.Item2 - 1) ||
                            (c.Item1 == t.Item1 && c.Item2 == t.Item2 + 1)));

                    if (null == found)
                    {
                        return false;
                    }
                }
            }

            return true;
        }
    }
}
